#include "include/hal_pwm.h"
#include <iostream>
#include <string>
#include <unistd.h>

HAL::PWM_Master::PWM_Master(const std::string &dev, uint32_t freq, float duty)
{
    this->dev = dev;
    /*int fd = open(dev.c_str(), O_WRONLY);
    if (fd < 0)
    {
        std::cerr << "PWM_Master open " << dev << " failed" << std::endl;
    }
    close(fd);*/

    // /sys/class/pwm/pwmchip0/pwm1/
    if (access(this->dev.c_str(), F_OK)) 
    {
        auto [chip_num, pwm_num] = GetPWMChipNumAndPWMNum(this->dev);
        std::cout << "chip_num: " << chip_num << std::endl;
        std::cout << "pwm_num: " << pwm_num << std::endl;
        this->chipNum = chip_num;
        this->pwmNum = pwm_num;

        std::string pwm_path = "/sys/class/pwm/pwmchip" + std::to_string(chip_num) + "/export";
        int fd = open(pwm_path.c_str(), O_WRONLY);
        if (fd < 0)
        {
            std::cerr << "PWM_Master open " << pwm_path << " failed" << std::endl;
            return;
        }
        ssize_t ret = write(fd, std::to_string(pwm_num).c_str(), std::to_string(pwm_num).length());
        if (ret < 0)
        {
            std::cerr << "PWM_Master write " << pwm_path << " failed" << std::endl;
            close(fd);
            return;
        }
        close(fd);
    }

    this->SetPWMEnable(false);
    this->SetPWMPolarity(true);
    this->SetPWMFreq(freq);
    this->SetPWMDuty(duty);
    this->SetPWMEnable(true);
}

HAL::PWM_Master::~PWM_Master()
{
    this->SetPWMEnable(false);
    std::string pwm_path = "/sys/class/pwm/pwmchip" + std::to_string(this->chipNum) + "/unexport";
    int fd = open(pwm_path.c_str(), O_WRONLY);
    if (fd < 0)
    {
        std::cerr << "PWM_Master open " << pwm_path << " failed" << std::endl;
        return;
    }
    ssize_t ret = write(fd, std::to_string(this->pwmNum).c_str(), std::to_string(this->pwmNum).length());
    if (ret < 0)
    {
        std::cerr << "PWM_Master write " << pwm_path << " failed" << std::endl;
        close(fd);
        return;
    }
    close(fd);
}

std::pair<int, int> HAL::PWM_Master::GetPWMChipNumAndPWMNum(const std::string &path)
{
    int chip_number = -1;
    int pwm_number = -1;

    // 找到 pwmchip 的位置
    size_t chip_pos = path.find("pwmchip");
    if (chip_pos != std::string::npos) {
        size_t chip_start = chip_pos + 7; // 跳过 "pwmchip"
        size_t chip_end = path.find('/', chip_start);
        std::string chip_str = path.substr(chip_start, chip_end - chip_start);
        //std::cout << chip_str << std::endl;
        chip_number = std::stoi(chip_str);
    }

    // 找到 pwm 的位置
    size_t pwm_pos = path.find("pwm", chip_pos+7); // 从 pwmchip 之后找
    if (pwm_pos != std::string::npos) {
        size_t pwm_start = pwm_pos + 3; // 跳过 "pwm"
        size_t pwm_end = path.find('/', pwm_start);
        std::string pwm_str = path.substr(pwm_start, pwm_end - pwm_start);
        //std::cout << pwm_str << std::endl;
        pwm_number = std::stoi(pwm_str);
    }

    return {chip_number, pwm_number};
}

int HAL::PWM_Master::SetPWMFreq(uint32_t freq)
{
    this->basePeriod = 1000000000 / freq;
    return ConfigPWM("period", std::to_string(this->basePeriod));
}
int HAL::PWM_Master::SetPWMDuty(float duty)
{
    duty = (duty>100)?(100):(duty);
    duty = (duty<0)?(0):(duty);
    duty = this->basePeriod / (100/ duty);

    long duty_cycle = (long)duty;
    return ConfigPWM("duty_cycle", std::to_string(duty_cycle));
}
int HAL::PWM_Master::SetPWMEnable(bool enable)
{
    return ConfigPWM("enable", std::to_string(enable));
}
int HAL::PWM_Master::SetPWMPeriod(uint32_t period)
{
    this->basePeriod = period;
    return ConfigPWM("period", std::to_string(this->basePeriod));
}
int HAL::PWM_Master::SetPWMPolarity(bool polarity)
{
    // 0: high, 1: low
    if (polarity)
    {
        return ConfigPWM("polarity", "normal");
    }
    return ConfigPWM("polarity", "inversed");
}

int HAL::PWM_Master::ConfigPWM(const std::string &attr, const std::string &val)
{
    std::lock_guard<std::mutex> lock(mtx);
    std::string path = dev + "/" + attr;

    int fd = open(path.c_str(), O_WRONLY);
    if (fd < 0)
    {
        std::cerr << "PWM_Master ConfigPWM " << path << " failed" << std::endl;
    }

    int ret = write(fd, val.c_str(), val.size());
    if (ret < 0)
    {
        std::cerr << "PWM_Master ConfigPWM " << path << " failed" << std::endl;
    }
    close(fd);
    return ret;
}
